﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using System.Data;
using System.Text;
using System.Windows.Forms;
using RoverStudio.Common.Controller;

namespace CaterpillarRover
{
    public partial class ManualCaterpillarControllerUC : UserControl
    {
        private ITankController caterpillarController;
        private JoyStickController.JoystickController joystickController;

        //public CaterpillarController CaterpillarController
        //{
        //    get { return caterpillarController; }
        //    set { caterpillarController = value; }
        //}

        public ITankController CaterpillarController
        {
            get { return caterpillarController; }
            set { caterpillarController = value; }
        }


        private double Speed
        {
            get
            {
                return ((double)tbSpeed.Value / tbSpeed.Maximum);
            }
            set
            {
                if (value < 0 || value > 1)
                    return;
                tbSpeed.Value = Math.Max(tbSpeed.Maximum, (int)value * tbSpeed.Maximum);
            }
        }

        private double time = 0.5;

        public ManualCaterpillarControllerUC()
        {
            InitializeComponent();
            joystickController = null;
        }

        void joystickController_NewJoystickState(JoyStickController.JoyState state)
        {
            double left = (double)state.Y / 2.0 + (double)state.X / 2.0;
            double right = (double)state.Y / 2.0 - (double)state.X / 2.0;
            Console.WriteLine("left = " + left + "  right = " + right);
            try
            {
                left *= 2;
                right *= 2;
                left = Math.Max(-1, left);
                left =Math.Min(1, left);
                right = Math.Max(-1, right);
                right = Math.Min(1, right);
                MoveTank(left, right, 0.2);

            }
            catch { }
        }

        private void btnStop_Click(object sender, EventArgs e)
        {
            MoveTank(0, 0, 0);
        }

        private void btnF_Click(object sender, EventArgs e)
        {
            MoveTank(Speed, Speed, time);
        }

        private void btnB_Click(object sender, EventArgs e)
        {
            MoveTank(-Speed, -Speed, time);
        }

        private void btnR_Click(object sender, EventArgs e)
        {
            MoveTank(Speed, -Speed, time);
        }

        private void btnL_Click(object sender, EventArgs e)
        {
            MoveTank(-Speed, Speed, time);
        }

        private void btnFR_Click(object sender, EventArgs e)
        {
            MoveTank(Speed, 0, time);
        }

        private void btnFL_Click(object sender, EventArgs e)
        {
            MoveTank(0, Speed, time);
        }

        private void btnBR_Click(object sender, EventArgs e)
        {
            MoveTank(-Speed, 0, time);
        }

        private void btnBL_Click(object sender, EventArgs e)
        {
            MoveTank(0, -Speed, time);
        }


        private void cbJoystick_CheckedChanged(object sender, EventArgs e)
        {
        	
            try
            {
                if (cbJoystick.Checked)
                {
                    if (joystickController == null)
                    {
                        joystickController = new JoyStickController.JoystickController(100);
                        joystickController.NewJoystickState += new JoyStickController.JoystickStateDelegate(joystickController_NewJoystickState);

                    }
                    joystickController.Start();
                }
                else
                {
                    joystickController.Stop();
                }
            }
            catch
            {
            }
            
        }

        private void clbSwitchPeriferia_SelectedIndexChanged(object sender, EventArgs e)
        {
            /*
            UInt16 power = (UInt16)0;
            bool[] bits = new bool[16];
            for (int i = 0; i < clbSwitchPeriferia.CheckedIndices.Count; i++)
            {
                bits[clbSwitchPeriferia.CheckedIndices[i]] = true;
            }

            UInt16 pos = (UInt16)1;
            for (int i = 0; i < bits.Length; i++)
            {
                if (bits[i])
                {
                    power += pos;
                }
                pos *= 2;
            }

            caterpillarController.PowerPeriferia(power);

            */

        }

        private void btnGo_Click(object sender, EventArgs e)
        {
            try
            {
                double vLeft = Double.Parse(txtLeft.Text);
                double vRight = Double.Parse(txtRight.Text);
                double time = Double.Parse(txtTime.Text);
                MoveTank(vLeft, vRight, time);
            }
            catch
            {
                //TODO:
            }
        }

        private void btnTurnTower_Click(object sender, EventArgs e)
        {
            try
            {
                //caterpillarController.TurnTover(Int32.Parse(txtTurnTowerDegree.Text));
            }
            catch
            {
                MessageBox.Show("Integer value expected!");
            }
        }

        private void btnCollectECube_Click(object sender, EventArgs e)
        {
            //caterpillarController.CollectECube();
        }

        private void clbSwitchPeriferia_ItemCheck(object sender, ItemCheckEventArgs e)
        {
            UInt16 power = (UInt16)0;
            bool[] bits = new bool[16];
            for (int i = 0; i < clbSwitchPeriferia.CheckedIndices.Count; i++)
            {
                bits[clbSwitchPeriferia.CheckedIndices[i]] = true;
            }
            bits[e.Index] = (e.NewValue == CheckState.Checked);
            UInt16 pos = (UInt16)1;
            for (int i = 0; i < bits.Length; i++)
            {
                if (bits[i])
                {
                    power += pos;
                }
                pos *= 2;
            }

            //caterpillarController.PowerPeriferia(power);
        }

        private Dictionary<Keys, bool> keyStatus = new Dictionary<Keys, bool>();
        private Keys keyAccelrate = Keys.U;
        private Keys keyBrake = Keys.J;
        private Keys keyLeft = Keys.H;
        private Keys keyRight = Keys.K;

        private double kbSpeed = 0;
        private double kbTurn = 0;
        
        private void btnStop_KeyDown(object sender, KeyEventArgs e)
        {
            keyStatus[e.KeyCode] = true;
        }

        private void btnStop_KeyUp(object sender, KeyEventArgs e)
        {
            keyStatus[e.KeyCode] = false;
        }

        private void cbKeyboard_CheckedChanged(object sender, EventArgs e)
        {
            keyStatus[keyAccelrate] = false;
            keyStatus[keyBrake] = false;
            keyStatus[keyLeft] = false;
            keyStatus[keyRight] = false;
            timerKeyboardControl.Enabled = cbKeyboard.Checked;
            timerKeyboardControl.Interval = 100;
            kbSpeed = 0;
            kbTurn = 0;
        }

        private void timerKeyboardControl_Tick(object sender, EventArgs e)
        {
            double maxSpeed = 0.5;
            double accel = 1.0/8;
            double turnAccel = 1.0/8;
            double maxTurn = 0.5;


            if (keyStatus[keyAccelrate] && kbSpeed < maxSpeed)
                kbSpeed = Math.Min(maxSpeed, kbSpeed + accel);
            else if (keyStatus[keyBrake] && kbSpeed > -maxSpeed)
                kbSpeed = Math.Max(-maxSpeed, kbSpeed - accel);
            else
            {
                kbSpeed -= Math.Sign(kbSpeed) * accel;
                if (Math.Abs(kbSpeed) < accel)
                    kbSpeed = 0;
            }

            kbTurn *= 0.9;
            if (keyStatus[keyLeft] && kbTurn < maxTurn)
                kbTurn = Math.Min(maxTurn, kbTurn + turnAccel);
            if (keyStatus[keyRight] && kbTurn > -maxTurn)
                kbTurn = Math.Max(-maxTurn, kbTurn - turnAccel);
            double left = kbSpeed - kbTurn;
            double right = kbSpeed + kbTurn;
            MoveTank(left, right, 0.3);
            Console.WriteLine("left = " + left + "  right = " + right + "  speed = "+kbSpeed);
        }



        private void MoveTank(double left, double right, double time)
        {
            if (caterpillarController == null)
                return;
            caterpillarController.MoveTank(-left, right, time);
            ShowOutput(-left, right);
        }

        private delegate void ShowOutputDelegate(double left, double right);
        private void ShowOutput(double left, double right)
        {
            if (pbLeft.InvokeRequired)
                pbLeft.Invoke(new ShowOutputDelegate(ShowOutput), left, right);
            else
            {
                pbLeft.Value = Math.Max(0, Math.Min(200, (int)((1 + left) * 100)));
                pbRight.Value = Math.Max(0, Math.Min(200, (int)((1 + right) * 100)));
                tbLeft.Value = Math.Max(0, Math.Min(200, (int)((1 + left) * 100)));
                tbRight.Value = Math.Max(0, Math.Min(200, (int)((1 + right) * 100)));
            }
        }
    }
}
